import matplotlib.pyplot as plt

font2 = {'family' : 'Times New Roman', 'weight' : 'normal', 'size' : 12}

def drawPath(traj, draw_state, Point_O):
    # fig1 = plt.figure()
    # plt.title("处理后轨迹散点图")
    # plt.scatter(traj.x, traj.y, s=4, color = "b")
    # # 画出原始路径尖点
    # for i in range(len(Point_O)):
    #     plt.scatter(Point_O[i][0], Point_O[i][1], s=18, color='m') 
    # # 画出处理后路径尖点
    # direct = traj.direction[0]
    # for i in range(len(traj.x)):
    #     if traj.direction[i] is not direct:
    #         direct = traj.direction[i]
    #         plt.scatter(traj.x[i - 1], traj.y[i - 1], s=18, color='r')
    # plt.xlabel('x(m)',font2)
    # plt.ylabel('y(m)',font2)
    # plt.grid() # 显示网格
    # plt.legend(loc = 'upper right', fontsize=12)  # 将图例显示在右上方
    # plt.axis("equal") # 将 x 轴和 y 轴的比例设置为相等

    fig2 = plt.figure()
    # 设置横纵坐标的名称以及对应字体格式
    plt.title("轨迹对比图")
    plt.plot(draw_state.x, draw_state.y, color = "r", marker = ".", label = "真实轨迹")
    plt.plot(traj.x, traj.y, color = "b", linewidth = 2, label = "参考轨迹")    
    plt.xlabel('x(m)',font2)
    plt.ylabel('y(m)',font2)
    plt.grid() # 显示网格
    plt.legend(loc = 'upper right', fontsize=12)  # 将图例显示在右上方
    plt.axis("equal") # 将 x 轴和 y 轴的比例设置为相等
    
    # fig3 = plt.figure()
    # plt.title("kappa")
    # plt.plot(traj.kappa, color='m', label='曲率')

    plt.show()
